import time

from robomaster import conn
from multi_robomaster import multi_robot
from robomaster import led, blaster
import pos_nokov as pos
import sympy as sp
import math
import utils
import random

field_range=[[-2100,2400],[-1300,1400]]
power=0
global poses, pos_ball

def led_red_solid(robot_group):
    """红灯常亮"""
    robot_group.led.set_led(led.COMP_ALL, 255, 1, 1, led.EFFECT_ON)

def led_grey_solid(robot_group):
    """grey常亮"""
    robot_group.led.set_led(led.COMP_ALL, 75, 255, 255, led.EFFECT_ON)

def get_pass_posi(pos_ball, pos_mate):
    bx=pos_ball[0]
    by=pos_ball[1]
    mx=pos_mate[0]
    my=pos_mate[1]
    x,y=sp.symbols('x, y')
    solutions=sp.nonlinsolve([(x-bx)*(by-my)-(y-by)*(bx-mx),
                              (x-bx)**2+(y-by)**2-250000],[x,y])
    # print(solutions)
    for s in solutions:
        if not ((bx<s[0]<mx or bx>s[0]>mx) and (by<s[1]<my or by>s[1]>my)):
            posi=s
    return posi

def get_pass_ori(pos_ball, pos_mate):
    bx=pos_ball[0]
    by=pos_ball[1]
    mx=pos_mate[0]
    my=pos_mate[1]
    ori=math.atan2(my-by, mx-bx)*180/math.pi
    return ori
    
def move_to_ready(robot_group):
    global power
    posi=get_pass_posi(pos_ball, poses[1-power][1])
    posi_target=utils.trans_relative_co(poses[power], posi)
    print('========move to:(y make a negative)')
    print(posi_target)
    robot_group.chassis.move(posi_target[0]/1000*1.1,-posi_target[1]/1000*1.1,0,0.7).wait_for_completed()

def move_to_follow(robot_group):
    global power
    posi=get_pass_posi(pos_ball, poses[1-power][1])
    print(posi)
    posi_target=utils.trans_relative_co(poses[power], posi)
    ori=get_pass_ori(pos_ball, poses[1-power][1])
    z=ori-poses[power][0]
    
    if z>180:
        z=z-360
    elif z<-180:
        z=z+360
    print('##### rotate: %.3f' % z)
    print('x: % .3f, y: % .3f, z: % .1f' % (posi_target[0]/1000,-posi_target[1]/1000,z))
    robot_group.chassis.drive_speed(x=posi_target[0]/100,y=-posi_target[1]/100,z=-z,timeout=2) #z的方向，speed和move相反
    # time.sleep(1)
    
    
def rotate_to_ready(robot_group):
    global power
    ori=get_pass_ori(pos_ball, poses[1-power][1])
    z=ori-poses[power][0]
    print('##### rotate: %.3f' % z)
    if z>180:
        z=z-360
    elif z<-180:
        z=z+360
    robot_group.chassis.move(0,0,z,90).wait_for_completed()

def catch(robot_group):
    ep_arm = robot_group.robotic_arm
    ep_chassis = robot_group.chassis
    ep_gripper=robot_group.gripper
    ep_gripper.open(power=50)
    time.sleep(2)
    ep_gripper.pause()
    ep_chassis.move(0.23, 0, 0, 0.5).wait_for_completed()
    ep_gripper.close(power=50)
    time.sleep(2)
    ep_gripper.pause()
    ep_arm.move(x=0, y=20).wait_for_completed()
    
def move_to_shoot(robot_group):
    robot_group.chassis.move(0,(random.random()-0.5),0,1.5).wait_for_completed()

def shoot(robot_group):
    ep_arm = robot_group.robotic_arm
    ep_chassis = robot_group.chassis
    ep_gripper=robot_group.gripper
    ep_arm.move(x=0, y=-20).wait_for_completed()
    ep_gripper.open(power=50)
    time.sleep(2)
    ep_gripper.pause()
    ep_chassis.move(-0.10, 0, 0, 1.8).wait_for_completed()
    ep_chassis.move(0, 0, 90, 90).wait_for_completed()
    ep_chassis.move(0, 0.5, 0, 1.3).wait_for_completed()
    ep_chassis.move(0, -1, 0, 2).wait_for_completed()
    ep_chassis.move(0, 0, -90, 90).wait_for_completed()
    ep_arm.move(x=0, y=-10).wait_for_completed()   
    
def back(robot_group):
    robot_group.chassis.move(-0.5,0,0,2).wait_for_completed()
    

def run():
    global poses, pos_ball, power
    # robomaster.config.LOCAL_IP_STR = "192.168.1.111"
    robots_sn_list = ['3JKDH2T00159G8','3JKCJC400302GS','3JKCJC400301ZP', 
                      '3JKCJC400301UD','3JKCJC400301W0', '3JKCJC400300Y9']

    multi_robots = multi_robot.MultiEP()
    multi_robots.initialize()

    number = multi_robots.number_id_by_sn([2, robots_sn_list[1]], [3, robots_sn_list[2]])
    print("The number of robot is: {0}".format(number))
    g_all = multi_robots.build_group([2, 3])
    g_red = multi_robots.build_group([2])
    g_grey = multi_robots.build_group([3])
    groups=[g_red, g_grey]
    multi_robots.run([g_grey, led_grey_solid])
    multi_robots.run([g_red, led_red_solid])
    print(power)
    while(input("robot loop: Press q to quit\n") != "q"):
        # poses=[pos.get_device_pose(1),pos.get_device_pose(2)]
        # print(poses)
        
        # multi_robots.run([groups[power],move_to_ready])
        # poses=[pos.get_device_pose(1),pos.get_device_pose(2)]

        # multi_robots.run([groups[power],rotate_to_ready])
        # multi_robots.run([groups[power],catch])
        # multi_robots.run([groups[power],move_to_shoot])
        
        # poses=[pos.get_device_pose(1),pos.get_device_pose(2)]
        # print('#3 poses ###########')
        # print(poses)
        # multi_robots.run([groups[power],rotate_to_ready])
        # multi_robots.run([groups[power],shoot])
        # multi_robots.run([groups[power],back])
        for i in range(3):
            print('#'*8)
            poses=[pos.get_device_pose(1),pos.get_device_pose(2)]
            pos_ball=pos.get_ball_pose()
            print(poses)
            multi_robots.run([groups[power],move_to_follow])
            time.sleep(0.5)
            
        print('###############completed series acitons')
        
        
        
        # power=1-power
        
    
    multi_robots.close()
    